/*
 * level1.c
 *
 * Copyright (c) 2009, Eugen Stoian <stoian.e@gmail.com>
 * 
 * This library is free software; you can redistribute it and/or modify it under
 * the terms of the GNU Lesser General Public License as published by the Free
 * Software Foundation; either version 2.1 of the License, or (at your option)
 * any later version.
 * 
 * This library is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 * FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
 * details.
 * 
 * You should have received a copy of the GNU Lesser General Public License
 * along with this library; if not, write to the Free Software Foundation, Inc.,
 * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA
 * http://www.fsf.org/licensing/licenses/lgpl.txt
 */


#include <level1.h>
#include <pxxdefs.h>
#include <emmintrin.h>
#include <math.h>


/*
 * SUM
 */
void ssum(int n, const float* x, const float* y, float* result){
	int i;
	__m128 rx;
	__m128 ry;
	__m128 rr;
	for(i = 0; i < n; i+=4) {
		rx = _mm_load_ps(x+i);
		ry = _mm_load_ps(y+i);
		rr = _mm_add_ps(rx,ry);
		_mm_store_ps(result+i,rr);
	}
}
void dsum(int n, const double* x, const double* y, double* result){
	int i;
	__m128d rx;
	__m128d ry;
	__m128d rr;
	for(i = 0; i < n; i+=2) {
		rx = _mm_load_pd(x+i);
		ry = _mm_load_pd(y+i);
		rr = _mm_add_pd(rx,ry);
		_mm_store_pd(result+i,rr);
	}

}/*
 * ASUM
 */
void sasum(int n, const float* x, const float* y, float* result){
	int i;
	int  __attribute__((aligned(16))) clrsgn[4] = {0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF,0x7FFFFFFF};
	__m128 rx;
	__m128 ry;
	__m128 rmask = _mm_load_ps((float*)clrsgn);
	for(i = 0; i < n; i+=4){
		rx = _mm_and_ps(rmask,_mm_load_ps(x+i));
		ry = _mm_and_ps(rmask,_mm_load_ps(y+i));
		_mm_store_ps(result+i,_mm_add_ps(rx,ry));
	}

}
void dasum(int n, const double* x, const double* y, double* result){
	int i;
	long __attribute__((aligned(16))) clrsgn[2] = {0x7FFFFFFFFFFFFFFF, 0x7FFFFFFFFFFFFFFF};
	__m128d rx;
	__m128d ry;
	__m128d rmask = _mm_load_pd((double*)clrsgn);
	for(i = 0; i < n; i+=2){
		rx = _mm_and_pd(rmask,_mm_load_pd(x+i));
		ry = _mm_and_pd(rmask,_mm_load_pd(y+i));
		_mm_store_pd(result+i,_mm_add_pd(rx,ry));
	}
}
/*
 * DOT PRODUCT
 */
float sdot(int n, const float* x, const float* y){
	float result;
	int i;
	__m128 rx;
	__m128 ry;
	__m128 rp;
	__m128 rr = _mm_set1_ps(0.0f);
	for(i = 0; i < n; i+=4){
		rx = _mm_load_ps(x+i);
		ry = _mm_load_ps(y+i);
		rp = _mm_mul_ps(rx,ry);
		rr = _mm_add_ps(rr,rp);
	}
	__m128 rrt = _mm_shuffle_ps(rr,rr,_MM_SHUFFLE(1,0,3,2));
	rr = _mm_add_ps(rr,rrt);
	rrt = _mm_shuffle_ps(rr,rr,_MM_SHUFFLE(0,1,2,3));
	rr = _mm_add_ps(rr,rrt);
	_mm_store_ss(&result,rr);
	return result;
}

double ddot(int n, const double* x, const double* y){
	double result;
	int i;
	__m128d rx;
	__m128d ry;
	__m128d rp;
	__m128d rr = _mm_set1_pd(0.0);
	for(i = 0; i < n; i+=2){
		rx = _mm_load_pd(x+i);
		ry = _mm_load_pd(y+i);
		rp = _mm_mul_pd(rx,ry);
		rr = _mm_add_pd(rr,rp);
	}
	__m128d rrt = _mm_shuffle_pd(rr,rr,1);
	rr = _mm_add_pd(rr,rrt);
	_mm_store_sd(&result,rr);
	return result;
}

/*
 * EUCLIDEAN NORM
 */
float snrm2(int n, const float* x){
	return sqrtf(sdot(n,x,x));
}
double dnrm2(int n, const double* x){
	return sqrt(ddot(n,x,x));
}

/*
 * y <- a*x+y
 */
void saxpy(int n, float a, const float* x, float* y){
	int i;
	for(i=0;i<n;++i){
		y[i]+=a*x[i];
	}
}
void daxpy(int n, float a, const double* x, double* y){
	int i;
	for(i=0;i<n;++i){
		y[i]+=a*x[i];
	}

}

/*
 * x <- a*x
 */
void sscale(int n, float a, float* x){
	int i;
	__m128 ra = _mm_set1_ps(a);
	__m128 rx;
	for(i=0;i<n;i+=4){
		rx = _mm_load_ps(x+i);
		rx = _mm_mul_ps(ra,rx);
		_mm_store_ps(x+i,rx);
	}
}
void dscale(int n, double a, double* x){
	int i;
	__m128d ra = _mm_set1_pd(a);
	__m128d rx;
	for(i=0;i<n;i+=2){
		rx = _mm_load_pd(x+i);
		rx = _mm_mul_pd(ra,rx);
		_mm_store_pd(x+i,rx);
	}
}

/*
 * IAMAX
 */
int samax(int n, const float* x){
	int  __attribute__((aligned(16))) clrsgn[4] = {0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF,0x7FFFFFFF};
	__m128 rmask = _mm_load_ps((float*)clrsgn);
	__m128 rcmp;
	__m128 rx;
	__m128 rmax = _mm_set1_ps(-1.0f);
	__m128i idxcmp, idx, idxmax;
	idxmax = _mm_set_epi32(3,2,1,0);
	int i = 0;
	for(;i<n;i+=4){
		idx = _mm_set_epi32(i+3,i+2,i+1,i);
		rx = _mm_load_ps(x+i);
		/*
		 * Absolute value
		 */
		rx = _mm_and_ps(rmask,rx);
		/*
		 * Compare
		 */
		rcmp = _mm_cmplt_ps(rmax,rx);
		idxcmp = _mm_castps_si128(rcmp);
		/*
		 * Find and store max values and indexes
		 */
		rmax = _mm_andnot_ps(rcmp,rmax);
		rx = _mm_and_ps(rcmp,rx);
		rmax = _mm_or_ps(rx,rmax);
		idxmax = _mm_andnot_si128(idxcmp,idxmax);
		idx = _mm_and_si128(idxcmp,idx);
		idxmax = _mm_or_si128(idxmax,idx);
	}
	idx = _mm_shuffle_epi32(idxmax,_MM_SHUFFLE(1,0,3,2));
	rx = _mm_shuffle_ps(rmax,rmax,_MM_SHUFFLE(1,0,3,2));
	rcmp = _mm_cmplt_ps(rmax,rx);
	idxcmp = _mm_castps_si128(rcmp);
	rmax = _mm_andnot_ps(rcmp,rmax);
	rx = _mm_and_ps(rcmp,rx);
	idxmax = _mm_andnot_si128(idxcmp,idxmax);
	idx = _mm_and_si128(idxcmp,idx);
	rmax = _mm_or_ps()
}

int damax(int n, const double* x){
	struct idx {
		int i0;
		int i1;
	};
	double amax = 0.0;
	int imax = 0;
	int i;
	for(i = 0; i < n; ++i)
		if(amax<abs(x[i])){
			amax = abs(x[i]);
			imax = i;
		}
	return i;
}

